import cv2
import numpy as np
import glob

# 设置棋盘格大小
chessboard_size = (9, 6)  # 内角的数目
square_size = 40  # 方格大小，单位是 mm

# 世界坐标系中的三维点（棋盘格角点的 3D 坐标）
objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)
objp = objp * square_size

# 存储所有图片中的对象点和图像点
objpoints = []
imgpoints = []

# 读取所有图片
images = glob.glob('/home/qb/qb_2021111239/qb_2021111239/week04/work7/工业相机照片/*.bmp')  # 修改为你的文件路径

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # 寻找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

    # 如果找到足够的角点，添加对象点和图像点
    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

        # 画出角点
        img = cv2.drawChessboardCorners(img, chessboard_size, corners, ret)
        cv2.imshow('img', img)
        cv2.waitKey(100)

cv2.destroyAllWindows()

# 进行相机标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

# 输出内参矩阵和畸变参数
print("Camera intrinsic matrix:\n", mtx)
print("Distortion coefficients:\n", dist)

# 计算投影误差
total_error = 0
for i in range(len(objpoints)):
    imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
    error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
    total_error += error
print("Mean reprojection error: ", total_error / len(objpoints))
